
#include "bits/stdc++.h"

#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"

using namespace std;

Eigen::Matrix3d skew(Eigen::Vector3d _in){
    Eigen::Matrix3d res;
    res << 0., -_in(2), _in(1),
            _in(2), 0., -_in(0),
            -_in(1), _in(0), 0.;
    return res;
}


Eigen::Vector3d func(Eigen::Quaterniond _r1, Eigen::Quaterniond _r2, Eigen::Vector3d _p){
    return _r1 * _r2.conjugate() * _p;
}



int main(void){

    // jacobian 矩阵是 3 × （3 + 3 + 3）  维度的 
    Eigen::AngleAxisd rotation_vector1(0.8, Eigen::Vector3d(0.3, 0.4, 0.2).normalized());
    Eigen::Quaterniond r1(rotation_vector1);
    Eigen::AngleAxisd rotation_vector2(0.8, Eigen::Vector3d(0.1, 0.4, 0.4).normalized());
    Eigen::Quaterniond r2(rotation_vector2);
    r2 = r2.normalized();
    Eigen::Vector3d p(1, 1, 1);

    std::cout << "手动求解雅可比矩阵" << std::endl;
    std::cout << "df/dr1 = " << std::endl << -r1.matrix() * skew(r2.conjugate() * p) << std::endl;
    std::cout << "df/dr2 = " << std::endl << r1.matrix() * skew(r2.conjugate() * p) << std::endl;
    std::cout << "df/dp = " << std::endl << r1.matrix() * r2.conjugate().matrix() << std::endl;

    Eigen::Vector3d res = func(r1, r2, p);
    
    double eps = 1e-8;
    Eigen::Vector3d inc0(eps, 0, 0);
    Eigen::Vector3d inc1(0, eps, 0);
    Eigen::Vector3d inc2(0, 0, eps);

    // 对 r1 的数值
    Eigen::Matrix3d dfdr1;
    Eigen::Vector3d resDelta = func(r1 * Eigen::Quaterniond(1., inc0[0]/2, inc0[1]/2, inc0[2]/2).normalized(), r2, p);
    dfdr1.block<3, 1>(0, 0) = (resDelta - res)/eps;
    resDelta = func(r1 * Eigen::Quaterniond(1., inc1[0]/2, inc1[1]/2, inc1[2]/2).normalized(), r2, p);
    dfdr1.block<3, 1>(0, 1) = (resDelta - res)/eps;
    resDelta = func(r1 * Eigen::Quaterniond(1., inc2[0]/2, inc2[1]/2, inc2[2]/2).normalized(), r2, p);
    dfdr1.block<3, 1>(0, 2) = (resDelta - res)/eps;

    // 对 r2 的数值
    Eigen::Matrix3d dfdr2;
    resDelta = func(r1, r2 * Eigen::Quaterniond(1., inc0[0]/2, inc0[1]/2, inc0[2]/2).normalized(), p);
    dfdr2.block<3, 1>(0, 0) = (resDelta - res)/eps;
    resDelta = func(r1, r2 * Eigen::Quaterniond(1., inc1[0]/2, inc1[1]/2, inc1[2]/2).normalized(), p);
    dfdr2.block<3, 1>(0, 1) = (resDelta - res)/eps;
    resDelta = func(r1, r2 * Eigen::Quaterniond(1., inc2[0]/2, inc2[1]/2, inc2[2]/2).normalized(), p);
    dfdr2.block<3, 1>(0, 2) = (resDelta - res)/eps;

    // 对 p 的数值
    Eigen::Matrix3d dfdp;
    resDelta = func(r1, r2, p + inc0);
    dfdp.block<3, 1>(0, 0) = (resDelta - res)/eps;
    resDelta = func(r1, r2, p + inc1);
    dfdp.block<3, 1>(0, 1) = (resDelta - res)/eps;
    resDelta = func(r1, r2, p + inc2);
    dfdp.block<3, 1>(0, 2) = (resDelta - res)/eps;

    std::cout << "数值雅可比矩阵" << std::endl;
    std::cout << "df/dr1 = " << std::endl << dfdr1 << std::endl;
    std::cout << "df/dr2 = " << std::endl << dfdr2 << std::endl;
    std::cout << "df/dp = " << std::endl << dfdp << std::endl;

    

    return 0;
}